/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "clkpointdetector.h"

CLKPointDetector::CLKPointDetector( const IplImage *sample_ )
{

	prevImage = 0;
	pyramid1 = pyramid2 = 0;
	result = 0;
	//store sample image in internal buffer
	sample = cvCloneImage( sample_ );
	foundFeatures[0] = 0;
	foundFeatures[1] = 0;
	foundFeatures[2] = 0;
	foundFeatures[3] = 0;
	threshold = 1e+4;
}

CLKPointDetector::~CLKPointDetector()
{
	if(sample) cvReleaseImage(&sample);
	if(prevImage) cvReleaseImage(&prevImage);
	if(pyramid1) cvReleaseImage(&pyramid1);
	if(pyramid2) cvReleaseImage(&pyramid2);
	if(result) cvReleaseImage(&result);
}

bool CLKPointDetector::DetectPoint(const IplImage *img, cv::Point2f &point)
{
	//if this is the first received frame
	//store it in internal buffer and perform full search
	if(!prevImage || !foundFeatures[0]){
		//allocate memory for pyramids and previous frame
		if(!prevImage) prevImage = cvCloneImage(img);
		if(!pyramid1) pyramid1 = cvCloneImage(img);
		if(!pyramid2) pyramid2 = cvCloneImage(img);
		if(!result) result = cvCreateImage(cvSize(img->width-sample->width+1, img->height-sample->height+1), IPL_DEPTH_32F, 1);
		CvPoint minLoc;
		double min, max;
		
		//perform full-search
		//result is the matrix of L2 distances between image blocks and sample
		cvMatchTemplate( img, sample, result, CV_TM_SQDIFF );
		//select minimum from result
		cvMinMaxLoc(result, &min, &max, &minLoc, 0);
		point.x = minLoc.x + sample->width/2; 
		point.y = minLoc.y + sample->height/2;
		
		oldFeatures[0].x = minLoc.x;
		oldFeatures[0].y = minLoc.y;
		
		if(min > threshold){
			//feature hasn't been found
			foundFeatures[0] = 0;
			return false;
		}
		//feature has been found
		foundFeatures[0] = 1;
		return true;
	}else{
		CvTermCriteria terminationCriteria = cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 );
		//track features using optical flow
		cvCalcOpticalFlowPyrLK(prevImage, img, pyramid1, pyramid2, 
			oldFeatures, newFeatures, 1/*number of features*/, 
			cvSize(10,10)/*windows size is 10x10*/, 0/*one level pyramid*/, foundFeatures, 
			featuresError, terminationCriteria, 0/*no enhancements*/ );
			
		if(!foundFeatures[0]){
			//feature has been lost
			//try full-search
			CvPoint minLoc;
			double min, max;
			
			//perform full-search
			//result is the matrix of L2 distances between image blocks and sample
			cvMatchTemplate( img, sample, result, CV_TM_SQDIFF );
			//select minimum from result
			cvMinMaxLoc(result, &min, &max, &minLoc, 0);
			point.x = minLoc.x + sample->width/2; 
			point.y = minLoc.y + sample->height/2;
			
			oldFeatures[0].x = minLoc.x;
			oldFeatures[0].y = minLoc.y;
			
			if(min > threshold){
				//feature hasn't been found
				foundFeatures[0] = 0;
				return false;
			}
			//feature has been found
			foundFeatures[0] = 1;
			newFeatures[0].x = minLoc.x;
			newFeatures[0].y = minLoc.y;
		}
		point.x = newFeatures[0].x;
		point.y = newFeatures[0].y;
	}
	return true;
}
